PNEW.OLD[PNT,HE] - www.SailDart.org

perm filename PNEW.OLD[PNT,HE] blob sn#543612 filedate 1980-10-28 generic text, type C, neo UTF8
COMMENT ⊗   VALID 00012 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	ENTRY
C00003 00003	!	SENSE, BIAS, COMPLY
C00007 00004	!	grasp
C00009 00005	!	open_hand 
C00010 00006	!	reach
C00012 00007	!	move
C00017 00008	!		release
C00018 00009	!		get
C00019 00010	!		put
C00020 00011	!	msmcall
C00023 00012	PROCEDURE XERROR
C00024 ENDMK
C⊗;
ENTRY;
BEGIN "PNEW"
COMMENT routines which are not available in AL;
DEFINE $PNEW=TRUE,$ALTER_EGO=TRUE;
REQUIRE "HEADER.SAI" SOURCE_FILE;

RPTR(EXPR$) PROCEDURE GETSCIFPOSSIBLE;
BEGIN
END;

STRING INPUT_STRING;
STRING CURRENTFRAME;


RPTR(EXPR$) PROCEDURE $GTIDREF(INTEGER TYPE; STRING S);
	BEGIN ! like $$gtidref except does not return sym ptr;
	RPTR(SYMBOL)SYM;
	RETURN($$GTIDREF(TYPE,SYM,S));
	END;
!	SENSE, BIAS, COMPLY;

PRELOAD_WITH "FX","FY","FZ","TX","TY","TZ";
STRING ARRAY FTYPE[1:6];
PRELOAD_WITH "FORCE(XHAT)","FORCE(YHAT)","FORCE(ZHAT)",
		"TORQUE(XHAT)","TORQUE(YHAT)","TORQUE(ZHAT)";
STRING ARRAY FSTYPE[1:6];

RECURSIVE PROCEDURE SENSE;
BEGIN	INTEGER I; RPTR(EXPR$)E; STRING S;
	S←" ON "; $CLNSAVE←NULL;
	GTOKEN;
	FOR I←1 STEP 1 UNTIL 6 DO IF EQU(FTYPE[I],TOKEN) THEN DONE;
	IF I>6 THEN ERROR("Need FX,FY,FZ,TX,TY,TZ here");
	S←S&FSTYPE[I];
	GTOKEN;
	IF TOKEN≠">" AND TOKEN≠"<" AND TOKEN≠"≥" AND TOKEN≠"≤" THEN
		ERROR("Need >,<,≥,≤ here");
	s←s&" "&TOKEN;
	$CLNSAVE←NULL;
	E←$$GTANYEXP("SENSE",#SC);
	S←S&" "&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&"DO ";
	$CLNSAVE←TOKEN;
	GTOKEN;
 	IF TOKENPTR≠NULL_RECORD AND SYMBOL:ACCESS[TOKENPTR]=#PROCEDURE
		THEN BEGIN PREF(TOKENPTR);
		S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]; $CLNSAVE←TOKEN;
		END
	ELSE IF EQU(TOKEN,"STOP")
		THEN BEGIN
			GTOKEN;
			IF EQU(TOKEN,"BARM") OR EQU(TOKEN,"YARM")
			THEN BEGIN S←S&" STOP "&TOKEN; GTOKEN; END
			ELSE S←S&" STOP "&CURRENTFRAME;
			STOKEN←TRUE;
		     END
	ELSE ERROR("REQUIRE A PROCEDURE HERE");
	INPUT_STRING←INPUT_STRING&S;
END;

RECURSIVE PROCEDURE BIAS;
BEGIN	INTEGER I; RPTR(EXPR$)E;
	STRING S;
	S←" WITH "; $CLNSAVE←NULL;
	GTOKEN;
	FOR I←1 STEP 1 UNTIL 6 DO IF EQU(FTYPE[I],TOKEN) THEN DONE;
	IF I>6 THEN ERROR("Need FX,FY,FZ,TX,TY,TZ here");
	S←S&FSTYPE[I];
	GTOKEN;
	IF TOKEN≠"=" THEN ERROR("Need = here");
	s←s&" "&TOKEN;
	$CLNSAVE←NULL;
	E←$$GTANYEXP("SENSE",#SC);
	S←S&" "&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&" ";
	$CLNSAVE←TOKEN;
	INPUT_STRING←INPUT_STRING&S;
END;

RECURSIVE PROCEDURE COMPLY;
BEGIN	
	STRING S; RPTR(EXPR$)E;
	S←" WITH STIFFNESS=(";
	$CLNSAVE←NULL;
	E←$$GTANYEXP("COMPLY",#VT);
	S←S&" "&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&",";
	$CLNSAVE←TOKEN;
	E←$$GTANYEXP("COMPLY",#VT);
	S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&") AT ";
	$CLNSAVE←TOKEN;
	E←$$GTANYEXP("COMPLY",#TR);
	S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&" ";
	$CLNSAVE←TOKEN;
	INPUT_STRING←INPUT_STRING&S;
END;
!	grasp;
PROCEDURE GRASP(INTEGER N);
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	IF N=1 THEN S←"GRASP1" ELSE IF N=2 THEN S←"GRASP2"
		ELSE ERROR("ONLY GRASP1 OR GRASP2 ALLOWED");
	S←S&"(";
	E←$$GTXP2;
	IF E=NULL_RECORD THEN S←S&"0,HAND_MAX);"
	    ELSE BEGIN
		S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&",";
		$CLNSAVE←TOKEN;
		E←$$GTXP2;
		IF E=NULL_RECORD THEN S←S&"HAND_MAX);"
			ELSE S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&");";
		END;
	INPUT_STRING←INPUT_STRING&S;
END;
!	open_hand ;
RECURSIVE PROCEDURE OPEN_HAND;
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"OPEN_HAND";
	GTOKEN; IF TOKEN="+" THEN S←S&"P" ELSE STOKEN←TRUE;
	S←S&"(";
	E←$$GTXP2;
	IF E THEN S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&");" ELSE S←S&"HAND_MAX);";
	INPUT_STRING←INPUT_STRING&S;
END;
!	reach;

!	bit position values:
for reach and moves:
	MM=3,MS=2,SM=1,SS=0, and F = add 4 ;

PRELOAD_WITH "REACHSS","REACHSM","REACHMS","REACHMM";
STRING ARRAY REACHNAME[0:4];
PRELOAD_WITH ";",NULL,";",NULL;
STRING ARRAY DEFAULT_END[0:4];
PRELOAD_WITH "PARK_POSITION","PARK_POSITION","PARK_POSITION","PARK_POSITION";
STRING ARRAY DEFAULT_DEST[0:4];
PRELOAD_WITH "DTOL","DTOL","DTOL","1000*INCHES";
STRING ARRAY DEFAULT_TOL[0:4];
PRELOAD_WITH "RTOL","RTOL","RTOL","RTOL";
STRING ARRAY DEFAULT_ATOL[0:4];

RECURSIVE PROCEDURE REACH(INTEGER N);
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←REACHNAME[N];
	GTOKEN; IF TOKEN="+" THEN S←S&"P" ELSE STOKEN←TRUE;
	S←S&"(";
	E←$$GTXP2;
	IF E=NULL_RECORD THEN S←S&DEFAULT_DEST[N]&","&DEFAULT_TOL[N]&","
			&DEFAULT_ATOL[N]&")"
	    ELSE BEGIN
		S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&",";
		$CLNSAVE←TOKEN;
		E←$$GTXP2;
		IF E=NULL_RECORD THEN S←S&DEFAULT_TOL[N]&","&DEFAULT_ATOL[N]&")"
		    ELSE BEGIN
			S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&",";
			$CLNSAVE←TOKEN;
			E←$$GTXP2;
			IF E=NULL_RECORD THEN S←S&DEFAULT_ATOL[N]&")"
			    ELSE S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&")";
			END;
		END;
	INPUT_STRING←INPUT_STRING&S&DEFAULT_END[N];
END;

!	move;

PRELOAD_WITH "MOVESS","MOVESM","MOVEMS","MOVEMM","FMOVESS","FMOVESM","FMOVEMS","FMOVEMM";
STRING ARRAY MOVENAME[0:7];
PRELOAD_WITH ";"," ",";"," "," "," "," "," ";
STRING ARRAY MOVEDEFAULT_END[0:7];
PRELOAD_WITH "PARK_POSITION","PARK_POSITION","PARK_POSITION","PARK_POSITION",
	     "PARK_POSITION","PARK_POSITION","PARK_POSITION","PARK_POSITION";
STRING ARRAY MOVEDEFAULT_DEST[0:7];
PRELOAD_WITH "DTOL","DTOL","DTOL","1000*INCHES","DTOL","DTOL","DTOL","DTOL";
STRING ARRAY MOVEDEFAULT_TOL[0:7];
PRELOAD_WITH "RTOL","RTOL","RTOL","RTOL","RTOL","RTOL","RTOL","RTOL";
STRING ARRAY MOVEDEFAULT_ATOL[0:7];

RECURSIVE PROCEDURE MOVE(INTEGER TYPE);
BEGIN
CASE TYPE OF
  BEGIN
  [0] [1] [2][3]
      BEGIN ! SS , SM ;
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←MOVENAME[TYPE];
	GTOKEN; IF TOKEN="+" THEN S←S&"P" ELSE STOKEN←TRUE;
	S←S&"(";
	IF TYPE=0 OR TYPE=1 THEN
	    BEGIN
	    E←$GTIDREF(#FR,"MSM MOVE COMMAND");
	    S←S&(CURRENTFRAME←$CLNSAVE[1 TO ∞-LENGTH(TOKEN)])&",";
	    $CLNSAVE←TOKEN;
	    END
	ELSE S←S&CURRENTFRAME&",";
	E←$$GTXP2;
	IF E=NULL_RECORD THEN S←S&MOVEDEFAULT_DEST[TYPE]&","&MOVEDEFAULT_TOL[TYPE]&","
			&MOVEDEFAULT_ATOL[TYPE]&")"
	    ELSE BEGIN
		S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&",";
		$CLNSAVE←TOKEN;
		E←$$GTXP2;
		IF E=NULL_RECORD THEN S←S&MOVEDEFAULT_TOL[TYPE]&","&MOVEDEFAULT_ATOL[TYPE]&")"
		    ELSE BEGIN
			S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&",";
			$CLNSAVE←TOKEN;
			E←$$GTXP2;
			IF E=NULL_RECORD THEN S←S&MOVEDEFAULT_ATOL[TYPE]&")"
			    ELSE BEGIN S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&")";
				$CLNSAVE←TOKEN;
				END;
			END;
		END;
	INPUT_STRING←INPUT_STRING&S&MOVEDEFAULT_END[TYPE];
      END;
  [4][5][6][7]
      BEGIN ! FSS,FSM ;
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←MOVENAME[TYPE];
	GTOKEN; IF TOKEN="+" THEN S←S&"P" ELSE STOKEN←TRUE;
	S←S&"(";
	IF TYPE=4 OR TYPE=5 THEN
	    BEGIN
	    E←$GTIDREF(#FR,"FSM OR FSS MOVE COMMAND");
	    S←S&(CURRENTFRAME←$CLNSAVE[1 TO ∞-LENGTH(TOKEN)])&",";
	    $CLNSAVE←TOKEN;
	    END
	ELSE S←S&CURRENTFRAME&",";
	E←$$GTXP2;
	IF E=NULL_RECORD THEN S←S&MOVEDEFAULT_DEST[TYPE]&")"
	    ELSE BEGIN
		S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&")";
		$CLNSAVE←TOKEN;
		END;
	INPUT_STRING←INPUT_STRING&S&MOVEDEFAULT_END[TYPE];
	GTOKEN;
	WHILE EQU(TOKEN,"SENSE") OR EQU(TOKEN,"BIAS") OR EQU(TOKEN,"COMPLY")
		DO BEGIN
			IF EQU(TOKEN,"SENSE") THEN SENSE
			ELSE IF EQU(TOKEN,"BIAS") THEN BIAS
			ELSE IF EQU(TOKEN,"COMPLY") THEN COMPLY;
			GTOKEN;
		   END;
	STOKEN←TRUE;
      END
  END;
END;
!		release;
PROCEDURE RELEASE;
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"RELEASE";
	E←$GTIDREF(#FR,"MSM RELEASE COMMAND");
	S←S&"("&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&",";
	$CLNSAVE←TOKEN;
	E←$$GTXP2;
	IF E THEN S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&");" ELSE S←S&"1.0*INCH);";
	INPUT_STRING←INPUT_STRING&S;
END;
!		get;
PROCEDURE GET;
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"GET";
	E←$GTIDREF(#FR,"MSM GET COMMAND");
	S←S&"("&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&",";
	$CLNSAVE←TOKEN;
	E←$$GTANYEXP("MSM GET COMMAND",#SC);
	IF E THEN S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&");" ELSE S←S&"1.0*INCH);";
	$CLNSAVE←TOKEN;
	INPUT_STRING←INPUT_STRING&S;
END;
!		put;
PROCEDURE PUT;
BEGIN
	RPTR(EXPR$)E; STRING S;
	$CLNSAVE←NULL;
	S←"PUT";
	E←$GTIDREF(#FR,"MSM PUT COMMAND");
	S←S&"("&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&",";
	$CLNSAVE←TOKEN;
	E←$$GTANYEXP("MSM PUT COMMAND",#TR);
	S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&",";
	$CLNSAVE←TOKEN;
	E←$$GTXP2;
	IF E THEN S←S&$CLNSAVE[1 TO ∞-LENGTH(TOKEN)]&");" ELSE S←S&"3.0*ZHAT*INCHES);";
	$CLNSAVE←TOKEN;
	INPUT_STRING←INPUT_STRING&S;
END;
!	msmcall;
INTERNAL PROCEDURE MSMCALL;
BEGIN
    BOOLEAN MM,RM;
    IF DEVICE≠DSK_X THEN ERROR("MSMCALL VALID ONLY FOR DISK INPUT AT THE MOMENT");
    IF $COMPILE≠0 THEN ERROR("VALID ONLY AT TOP LEVEL");
    INPUT_STRING←NULL;
    $CLNSAVE←NULL;
    MM←RM←FALSE;
    GTOKEN;
    WHILE NOT EQU(TOKEN,"MSMEND") DO
	BEGIN
	IF RM THEN
	    BEGIN
	    IF EQU(TOKEN,"RMM") THEN REACH(3)
	    ELSE IF EQU(TOKEN,"RMS") THEN BEGIN REACH(2); RM←FALSE; END
	    ELSE ERROR("NEED RMS OR RMM HERE");
	    END
	ELSE IF MM THEN
	    BEGIN
	    IF EQU(TOKEN,"MMM") THEN MOVE(3)
	    ELSE IF EQU(TOKEN,"MMS") THEN BEGIN MOVE(2); MM←FALSE; END
	    ELSE IF EQU(TOKEN,"FMM") THEN MOVE(7)
	    ELSE IF EQU(TOKEN,"FMS") THEN BEGIN MOVE(6); MM←FALSE; END
	    ELSE ERROR("NEED MMM,MMS,FMM OR FMS HERE");
	    END
	ELSE IF EQU(TOKEN,"G1") THEN GRASP(1)
	ELSE IF EQU(TOKEN,"G2") THEN GRASP(2)
	ELSE IF EQU(TOKEN,"RSS") THEN REACH(0)
	ELSE IF EQU(TOKEN,"RSM") THEN BEGIN REACH(1); RM←TRUE; END
	ELSE IF EQU(TOKEN,"MSS") THEN MOVE(0)
	ELSE IF EQU(TOKEN,"MSM") THEN BEGIN MOVE(1); MM←TRUE; END
	ELSE IF EQU(TOKEN,"FSS") THEN MOVE(4)
	ELSE IF EQU(TOKEN,"FSM") THEN BEGIN MOVE(5); MM←TRUE; END
	ELSE IF EQU(TOKEN,"OPN")  THEN OPEN_HAND
	ELSE IF EQU(TOKEN,"RMM") OR EQU(TOKEN,"RMS")
		THEN ERROR("RMM,RMS can only follow a RSM or RMM")
	ELSE IF	EQU(TOKEN,"MMM") OR EQU(TOKEN,"MMS") OR
		EQU(TOKEN,"FMM") OR EQU(TOKEN,"FMS")
		THEN ERROR("MMM,MMS,FMM,FMS can only follow a MSM,MMM,FSM,FMM")
	ELSE IF EQU(TOKEN,"RELEASE") THEN RELEASE
	ELSE IF EQU(TOKEN,"GET") THEN GET
	ELSE IF EQU(TOKEN,"PUT") THEN PUT
	ELSE INPUT_STRING←INPUT_STRING&$CLNSAVE;
	IF NOT STOKEN THEN $CLNSAVE←NULL;
	GTOKEN;
	END;
    SEMICOL_READ; STOKEN←TRUE;
    $CLNSAVE←NULL;
    ASKUSER(INPUT_STRING);
END;
PROCEDURE XERROR;
	ERROR(TOKEN," is a dummy statement, use some other word");

INTERNAL PROCEDURE FOOCALL(INTEGER I);
	XERROR;

END "PNEW"